/*
 * File including the Conjugate Gradient functions in additions
 * to the bisection and bracketing functions, in addition to some
 * secondary functions.
 * Salem Machaka
 * 04/14/98
 */


#include "mle.h"


/*
 * Steepest descent algorithm
 * Given a vector y, move to the minimum 
 * along the gradient direction 
 */
double Conj(double *y, int nprobe, int nclone, double **aa, 
		int *order, int M, double const1, double *R)
{

  int 		i,  dir;
  double 	old_value, new_value, shigh, slow, delta, gg, dgg, gam;
  double        *direction, *d1, *d2;



  /*
   * direction[nprobe+1] = - sum(direction[i]) for 1<=i<=n
   */
  direction = Dec_Vector(1,nprobe+1);
  d1 = Dec_Vector(1,nprobe+1);
  d2 = Dec_Vector(1,nprobe+1);


          
  
  new_value = old_value = Func(const1, R, aa, y, M, nprobe, nclone, order);
  
  Gradient(direction, y, aa, R, nprobe, nclone, M, order);
  
	
  direction[nprobe+1] = 0.0;
  for(i=1;i<=nprobe;i++)
   {
     direction[i] = d1[i] = d2[i] = -direction[i];
     direction[nprobe+1] -= direction[i];
     
   }
   
  while (1)
   {
	Project(direction, y, M, nprobe);
	
	
	
	dir = 0;
	for(i=1;i<=nprobe;i++){
	   if (!(direction[i] >= -0.0001 && direction[i] <= 0.0001)) dir = 1;
	}  
	
	if (dir == 0 ) break;
	Bracket(&shigh, &slow, y, direction, nprobe, M);
       	/*
         * Due to numerical errors, this has to be done in here.
         */
        if (shigh > 0) shigh -= 1.0e-10;
        
        
	Line_Minimize(shigh, slow, y, direction, nprobe, 
			nclone, aa, order, M, const1, R);
	
	
	
	old_value = new_value;
  	new_value = Func(const1, R, aa, y, M, nprobe, nclone, order);
	delta = old_value - new_value;
        /*
         * Checking that instead of delta < 0 because in Line_Minimization
         * the condition to exit bisection is of that order as well (actually
         * of a little bit higher order)
         */
        //if (delta < -1.0e-5) Abort("Error in Steepest Descent, not monotonically decreasing !\n");
	if (delta < CRITERIA_CONJ) break;
        Gradient(direction, y, aa, R, nprobe, nclone, M, order);
	gg = dgg = 0.0;
  	for(i=1;i<=nprobe;i++)
   	    {
		gg += d2[i]*d2[i];
		dgg += (direction[i]+d2[i])*direction[i];
 	    }
	gam = dgg/gg;
  	direction[nprobe+1] = 0.0;
  	for(i=1;i<=nprobe;i++){
		d2[i] = -direction[i];
		direction[i] = d1[i] = d2[i] + gam*d1[i];
     		direction[nprobe+1] -= direction[i];
        
	}
	    
	  
     }
	  
  

  Free_Dec(direction, 1);
  Free_Dec(d1, 1);
  Free_Dec(d2, 1);
  return new_value;

}



/*
 * function to bracket the minimum between shigh and slow
 */
void Bracket(double *shigh, double *slow, double *y, 
		double *direction, int nprobe, int M)
{

  int 		i;
  double 	t1;


  *shigh = DBL_MAX;
  *slow = 0.0;

  /*
   * The condition set on slow and shigh are derived from
   * the following:
   * 0 <= Yi <= M, slow >= 0 , shigh >= 0
   */
  for(i=1;i<=nprobe+1;i++)
	if (direction[i]>0)
	   {
		t1 = -y[i]/direction[i];
		if (*slow<t1)   *slow = t1;
		t1 = ((double)M -y[i])/direction[i];
		if (*shigh>t1)    *shigh = t1;
	   }
  	else if (direction[i]<0)
	   {
		t1 = ((double)M-y[i])/direction[i];
		if (*slow<t1)   *slow = t1;
		t1 = -y[i]/direction[i];
		if (*shigh>t1)    *shigh = t1;
	   }

  /*
   * Just checking to make sure there is no negative s
   * In that case slow should be 0 as well
   */
  if (*shigh<0) *shigh = 0.0;
    	
}




/*
 * Fuction to calculate the 1 dimensional derivative of the
 * function with respect to s
 */
double Gradient_S(double *y, double *direction, int nprobe, int nclone, double **aa,
			int *order, int M, double *R)
{

  int 		i,j;
  double	den, num, sum = 0.0;


  for(i=1;i<=nclone;i++)
    {
	den = 0.0;
        for(j=1;j<=nprobe+1;j++)
                den -= (1-aa[i][order[j]])*(1-aa[i][order[j-1]])*Min(y[j],(double)M);
        den += R[i];
	num = 0.0;
        for(j=1;j<=nprobe+1;j++){
                //if(isnan(y[j])) printf("y[j] is nan \n");
                num += (1-aa[i][order[j]])*(1-aa[i][order[j-1]])*Indicator(y[j],(double)M)*direction[j];
        }
    /* Fixed by Susanta if den ==0 then den = 0.001     */
    if(den == 0) den = 0.001;
	sum += num/den;
    }
  //fprintf(stdout,"num %lf \n",num);
  //if(isnan(num)) printf("big num is nan\n");
  return sum;

}
	

	
/*
 * Function to do line minimization with respect to s, given the 
 * direction, and the bracketing of the minimum.
 */
void  Line_Minimize(double shigh, double slow, double *y, double *direction, 
			int nprobe, int nclone, double **aa, int *order, int M, 
			double const1, double *R)
{
  int 		i;
  double 	scalar, old_func, new_func;
  double	grad_left, grad_right, grad_middle;
  double	*y_temp;
  int caseVble = 0;
     
  y_temp = Dec_Vector(1, nprobe+1);


  /*
   * Calculate the gradient at the 2 ends
   */
  for(i=1; i<=nprobe+1; i++){
   	y_temp[i] = y[i] + slow*direction[i];
   	//if(isnan(direction[i])) printf("direction[i] is nan \n");
  }
  
   	
  grad_left = Gradient_S(y_temp, direction, nprobe, nclone, aa, order, M, R);
  old_func = Func(const1, R, aa, y_temp, M, nprobe, nclone, order);

  for(i=1; i<=nprobe+1; i++)
   	y_temp[i] = y[i] + shigh*direction[i];
  grad_right = Gradient_S(y_temp, direction, nprobe, nclone, aa, order, M, R);
  
  while(1){
     for(i=1; i<=nprobe+1; i++){
       if(isnan(y[i])) printf("y[i] is nan \n");
       if(isnan(direction[i])) printf("direction[i] is nan \n");
     }
	/*
	 * Function monotonically decreasing from left to right
 	 */
	if ( grad_left<=0 && grad_right<=0 ) 
	   
	   {
	    caseVble = 1;
  		for(i=1; i<=nprobe+1; i++)
		    y[i] = y[i] + shigh*direction[i];
		break;
	   }
	/*
	 * Function monotonically decreasing from right to left
 	 */
	if ( grad_left>=0 && grad_right>=0 )
	   {
	    caseVble = 2;
  		for(i=1; i<=nprobe+1; i++)
		    y[i] = y[i] + slow*direction[i];
		break;
	   }
	/*
	 * Function in a cone shape, most generic case
	 */
	if ( (grad_left<=0 && grad_right>=0) || (grad_left>=0 && grad_right<=0) )
	   /* Modified by Susanta to include case:  grad_left>0 && grad_right<0 */
	   {
	    
	    
	     caseVble = 3;
		/*
		 * Calculating the gradient in the middle
		 */
		scalar = (slow+shigh)/2;	
  		for(i=1; i<=nprobe+1; i++)
   			y_temp[i] = y[i] + scalar*direction[i];
  		grad_middle = Gradient_S(y_temp, direction, nprobe, nclone, aa, order, M, R);
  		if(isnan(grad_middle)) printf("grad_middle is nan \n");
  		new_func = Func(const1, R, aa, y_temp, M, nprobe, nclone, order);
		/*
		 * If function value didn't change much but still 
		 * decreased, you've pretty much reached the minimum
		 */
		if ( (new_func <= old_func) && (old_func-new_func) <= CRITERIA_BISECTION )
		    {
		    caseVble = 4;
  			for(i=1; i<=nprobe+1; i++)
			    y[i] = y[i] + scalar*direction[i];
			break;
	  	    }
		/*
		 * otherwise, decrease interval from right side
		 */
		if (grad_middle>0) 
		   {
		    caseVble = 5;
			old_func = new_func;
			shigh = scalar;
			grad_right = grad_middle;
		   }
		/*
		 * otherwise, decrease interval from left side
		 */
		else 
		   {
		    caseVble = 6;
			old_func = new_func;
			slow = scalar;	
			grad_left = grad_middle;
		   }
		new_func = old_func;
	    }
	    //printf("grad_left: %lf  grad_right %lf  diff: %lf  caseVble : %d\n",grad_left,grad_right,(old_func-new_func),caseVble);
	//printf("I`m stuck here\n");   
    }
  //printf("going out \n");
  Free_Dec(y_temp,1);

}
